// this is the header file that tells the compiler what pins and ports, etc.
 // are available on this chip.
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <stdio.h>
 #include <stdint.h>
 #include "motorControl.h"
 #include "encoder.h"
 #include "sensors.h"
 
 // define what pins the LEDs are connected to.

 //#define Warn PD6
 //#define Red_L PD7
 #define Red PB2
 #define Yellow PB1
 #define Green PB0


 #define FOSC 20000000
 #define BAUD 9600
 #define MYUBRR (((FOSC/16)/BAUD)-1)

// Some macros that make the code more readable
 //#define output_low(port,pin) port &= (uint8_t)~(1<<pin)
 //#define output_high(port,pin) port |= (uint8_t)(1<<pin)
 //#define set_input(portdir,pin) portdir &= (uint8_t)~(1<<pin)
 //#define set_output(portdir,pin) portdir |=(uint8_t)(1<<pin)

 #define set(p,r) ((p) |= (bit(r)))
 #define clear(p,r) ((p) &= ~(bit(r)))
 #define bit(x) (0x01 << (x))
 
 #define startTimer1 set(TCCR1B,CS12)//256 prescale
 #define stopTimer1 clear(TCCR1B,CS12)
 
 void ioinit(void);      // initializes IO
 static int uart_putchar(char c, FILE *stream);

 uint8_t uart_getchar(void);

 static FILE mystdout = FDEV_SETUP_STREAM(uart_putchar, NULL, _FDEV_SETUP_WRITE);

 char turns = 0;

 // this is just a program that 'kills time' in a calibrated method
 void delay_ms(uint8_t ms) {
   uint16_t delay_count = F_CPU / 17500;
   volatile uint16_t i;
 
   while (ms != 0) {
     for (i=0; i != delay_count; i++);
     ms--;
   }
 }

 int main(void) {
   // initialize the direction of PORTD #6 to be an output
	
    ioinit(); //Setup IO pins and defaults  
	init_Motor();
	int_Encoders();
	sei();
	delay_ms(200);
	output_high(PORTB,Green);

	char temp;
	printf("Control software Demo Started\n");


  while(1){
  		output_high(PORTB,Yellow);
		delay_ms(100);
	//	printf("Moving, Turns = %d\n", turns);
  		
	
		
		if(turns >=2){
				lowBrake(10);
				delay_ms(100);
				stop(10);
				printf("Stop!, Turns = %d\n", turns);
				turns = 0;
				output_low(PORTB,Yellow);
		} 

	}//while(1)	
 }//main


void ioinit (void){
    set_output(DDRB, Yellow );
   	set_output(DDRB, Red);
   	set_output(DDRB, Green);

    //USART Baud rate: 9600
    UBRR0H = (MYUBRR>>8);
    UBRR0L = MYUBRR;
    UCSR0B = (1<<RXEN0)|(1<<TXEN0);
    
    stdout = &mystdout; //Required for printf init
}

static int uart_putchar(char c, FILE *stream){

    if (c == '\n'){
	loop_until_bit_is_set(UCSR0A, UDRE0);
    UDR0 = '\r';
	} 
 
    loop_until_bit_is_set(UCSR0A, UDRE0);
    UDR0 = c;
    
    return 0;
}

uint8_t uart_getchar(void){
    while( !(UCSR0A & (1<<RXC0)) );
    return(UDR0);
}


ISR(PCINT2_vect){
	turns++;

	}
